/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: Wheel Encoder Driver
 *         $Id: encoder.c 12 2014-04-10 03:35:56Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include <string.h>
#include "maxq_config.h"
#include "comm/maxq_comm.h"
#include "car.h"
#include "encoder.h"
#include "tick.h"

// capture mode, interrupt enabled, sysclk / 16
#define TIMER_CONFIG          0x020B
#define TIMER_FREQ            (XTAL_FREQ / 16)

#define MAX_TICKS_TO_SUM      16
#define MAX_TICKS_SHIFT       4

#define NUM_TICKS_CUTOFF      5000

#ifndef CAPTURE_BUFFER_SIZE
#define CAPTURE_BUFFER_SIZE   32
#endif

#define increment_capture_index(wheel) 

/***** Global Data *****/
volatile uint16_t last_capture[NUM_WHEELS];
volatile unsigned int overflow_cnt[NUM_WHEELS];
volatile unsigned int valid_cnt[NUM_WHEELS];
volatile int32_t odometer[NUM_WHEELS];
volatile unsigned int delta_index[NUM_WHEELS];
volatile uint16_t delta_buffer[NUM_WHEELS][CAPTURE_BUFFER_SIZE];
volatile unsigned int isr_overflow;
volatile unsigned int direction[NUM_WHEELS];

#define DELTA_BUFFER_SIZE   100
volatile unsigned int first_delta_index;
volatile uint16_t first_delta_buffer[DELTA_BUFFER_SIZE][NUM_WHEELS];
volatile unsigned int recent_delta_index;
volatile uint16_t recent_delta_buffer[DELTA_BUFFER_SIZE][NUM_WHEELS];

int32_t alarm_count[NUM_WHEELS];
void (*alarm_callback[NUM_WHEELS])(void);

unsigned int corrected = 0;

maxq_const uint16_t hundred_mm_per_cnt[NUM_WHEELS] = {
  (((WHEEL_CIRCUM_10MM_LEFT * 10) + (TICKS_PER_REV / 2)) / TICKS_PER_REV),
  (((WHEEL_CIRCUM_10MM_RIGHT * 10) + (TICKS_PER_REV / 2)) / TICKS_PER_REV),
};

maxq_const uint16_t wheel_circum_10mm[NUM_WHEELS] = {
  WHEEL_CIRCUM_10MM_LEFT,
  WHEEL_CIRCUM_10MM_RIGHT
};

maxq_const uint16_t mms7_per_cnt[NUM_WHEELS] = {
  (((WHEEL_CIRCUM_MMS4_LEFT << 3) + (TICKS_PER_REV >> 1)) / TICKS_PER_REV),
  (((WHEEL_CIRCUM_MMS4_RIGHT << 3) + (TICKS_PER_REV >> 1)) / TICKS_PER_REV),
};

/****************************************************************************/
int enc_init(void)
{
  unsigned int wheel;

  // Configure timers for capture
  TB0CN = TIMER_CONFIG;
  TB0CN |= 0x0004; // enable timer
  TB1CN = TIMER_CONFIG;
  TB1CN |= 0x0004; // enable timer

  for (wheel = 0; wheel < NUM_WHEELS; wheel++) {
    overflow_cnt[wheel] = 0xFFFF;
    odometer[wheel] = 0;
    delta_index[wheel] = 0;
    last_capture[wheel] = 0;
    valid_cnt[wheel] = 0;
    alarm_count[wheel] = 0;
    alarm_callback[wheel] = NULL;
    memset((void*)delta_buffer[wheel], 0xFF, sizeof(delta_buffer[wheel]));
    direction[wheel] = FORWARD;
  }

  enc_log_clear();

  isr_overflow = 0;

  return 1;
}

/****************************************************************************/
uint16_t getdelta(unsigned int wheel)
{
  uint16_t new_value, delta;

  __disable_interrupt();

  if (wheel == 0)
    new_value = TB0R;
  else
    new_value = TB1R;

  if (new_value > last_capture[wheel])
    delta = new_value - last_capture[wheel];
  else 
    delta = 0xFFFF - last_capture[wheel] + new_value + 1;

  __enable_interrupt();

  return delta;
}

/****************************************************************************/
unsigned int enc_get_avg(unsigned int wheel)
{
  int sample, index;
  uint32_t sum = 0;
  unsigned int howmany;
  unsigned int avg = 0xFFFF;
  uint16_t most_recent_sample, oldest_sample, current_delta;

  if (valid_cnt[wheel] > 1) {

    // Accumulate as many samples as we can up to max
    howmany = MAX_TICKS_TO_SUM;
    if (howmany > valid_cnt[wheel])
      howmany = valid_cnt[wheel];
  
    // Initialize index and most recent sample
    index = delta_index[wheel] - 1;
    if (index < 0) { index = CAPTURE_BUFFER_SIZE - 1; }
    most_recent_sample = delta_buffer[wheel][index];
  
    // Accumulate samples
    for (sample = 0; sample < howmany; sample++) {
      oldest_sample = delta_buffer[wheel][index];
      sum += oldest_sample;
      if (--index < 0) { index = CAPTURE_BUFFER_SIZE - 1; }
    }

    // Calculate average
    if (howmany == MAX_TICKS_TO_SUM)
      avg = sum >> MAX_TICKS_SHIFT;
    else 
      avg = sum / howmany;
  
    // Eliminate glitches in most recent sample
    if ( (avg > most_recent_sample) && (valid_cnt[wheel] > howmany) && ((avg - most_recent_sample) > (avg >> 3)) ) {
      sum -= most_recent_sample;
      sum += delta_buffer[wheel][index];
    }
  
    // Improve response time when slowing down
    // If we are slowing down, the current delta count value may have additional information
    current_delta = getdelta(wheel);
    if (current_delta > avg) {
      sum -= oldest_sample;
      sum += current_delta;
    }

    if (howmany == MAX_TICKS_TO_SUM)
      avg = sum >> MAX_TICKS_SHIFT;
    else
      avg = sum / howmany;
  }


  return avg;
}

/****************************************************************************/
unsigned int enc_get_speed(unsigned int wheel)
{
  uint32_t avg;
  unsigned int speed = 0;

  avg = enc_get_avg(wheel);
  if (avg < 0xFFFF)
    speed = (((uint32_t)TIMER_FREQ * wheel_circum_10mm[wheel]) / (avg * TICKS_PER_REV * 10));

  return speed;
}

/****************************************************************************/
void enc_alarm_set(unsigned int wheel, int32_t count, void (*func)(void))
{
  alarm_count[wheel] = odometer[wheel] + count;
  alarm_callback[wheel] = func;

  if (odometer[wheel] == alarm_count[wheel]) {
    if (alarm_callback[wheel] != NULL)
      alarm_callback[wheel]();
    alarm_callback[wheel] = NULL;
    alarm_count[wheel] = NO_ALARM;
  }
}

/****************************************************************************/
void enc_alarm_clr(unsigned int wheel)
{
  alarm_count[wheel] = NO_ALARM;
  alarm_callback[wheel] = NULL;
}

/****************************************************************************/
void enc_direction_change(unsigned int wheel, unsigned int new_direction)
{
  if (direction[wheel] == new_direction)
    return;

  if (valid_cnt[wheel] != 0)
    maxq_printf(C"ERROR: direction change when valid_cnt != 0\r\n");

  direction[wheel] = new_direction;
  valid_cnt[wheel] = 0;
}

/****************************************************************************/
void handle_capture(unsigned int wheel)
{
  int index;
  uint16_t delta;
  void (*temp_callback)(void);

  // If we already have a capture value
  if (valid_cnt[wheel] > 0) {

    delta = getdelta(wheel);
    index = delta_index[wheel];

    // This is most likely a valid new pulse
    delta_buffer[wheel][index] = delta;
    if (++delta_index[wheel] >= CAPTURE_BUFFER_SIZE) { delta_index[wheel] = 0; }

    if (first_delta_index < DELTA_BUFFER_SIZE) {
      first_delta_buffer[first_delta_index][wheel] = delta;
      first_delta_index++;
    }
  }

  // Recode last capture
  if (wheel == 0)
    last_capture[wheel] = TB0R;
  else
    last_capture[wheel] = TB1R;

  // Update the valid capture count
  if (valid_cnt[wheel] < 0xFFFF)
    valid_cnt[wheel]++;

  // Update the odometer depending on our direction of travel
  if (direction[wheel] == FORWARD)
    odometer[wheel]++;
  else
    odometer[wheel]--;

  // Reset the overflow count
  overflow_cnt[wheel] = 0;

  // Check the alarm
  if (odometer[wheel] == alarm_count[wheel]) {
    temp_callback = alarm_callback[wheel];
    alarm_callback[wheel] = NULL;
    alarm_count[wheel] = NO_ALARM;
    if (temp_callback != NULL)
      temp_callback();
  }
}

/****************************************************************************/
void enc_log_display(void)
{
  int i, l, r;

  l = delta_index[LEFT];
  r = delta_index[RIGHT];

  maxq_printf(C"\r\n***** Encoder Log *****\r\n");
  for (i = 0; i < CAPTURE_BUFFER_SIZE; i++, l++, r++) {
    if (l == CAPTURE_BUFFER_SIZE) { l = 0; }
    if (r == CAPTURE_BUFFER_SIZE) { r = 0; }
    maxq_printf("%3d %4x %4x\r\n", i, delta_buffer[LEFT][l], delta_buffer[RIGHT][r]);
  }
}

/****************************************************************************/
void enc_log_clear(void)
{
  memset((void*)first_delta_buffer, 0, sizeof(first_delta_buffer));
  first_delta_index = 0;
  memset((void*)recent_delta_buffer, 0, sizeof(recent_delta_buffer));
  recent_delta_index = 0;
}

/****************************************************************************/
void enc_log_display_first(void)
{
  int i;

  maxq_printf(C"\r\n***** First Encoder Log *****\r\n");
  for (i = 0; i < DELTA_BUFFER_SIZE; i++) {
    maxq_printf("%3d %4x %4x\r\n", i, first_delta_buffer[i][LEFT], first_delta_buffer[i][RIGHT]);
  }
}

/****************************************************************************/
void enc_log_display_recent(void)
{
  int i;

  maxq_printf(C"\r\n***** Recent Encoder Log *****\r\n");
  for (i = recent_delta_index; i < DELTA_BUFFER_SIZE; i++) {
    maxq_printf("%3d %4x %4x\r\n", i - recent_delta_index, recent_delta_buffer[i][LEFT], recent_delta_buffer[i][RIGHT]);
  }
  for (i = 0; i < recent_delta_index; i++) {
    maxq_printf("%3d %4x %4x\r\n", i + (DELTA_BUFFER_SIZE - recent_delta_index), recent_delta_buffer[i][LEFT], recent_delta_buffer[i][RIGHT]);
  }
}

/****************************************************************************/
#pragma vector = 6
__interrupt void timer_isr(void)
{
  // TB0 Overflow
  if (TB0CN & 0x80) {
    TB0CN &= ~0x80;
    if (overflow_cnt[0] < 3)
      overflow_cnt[0]++;
    if (overflow_cnt[0] == 2) {
      valid_cnt[0] = 0;
      delta_buffer[0][delta_index[0]] = 0xFFFF;
      if (++delta_index[0] >= CAPTURE_BUFFER_SIZE) { delta_index[0] = 0; }
    }
  }

  // TB1 Overflow
  if (TB1CN & 0x80) {
    TB1CN &= ~0x80;
    if (overflow_cnt[1] < 3)
      overflow_cnt[1]++;
    if (overflow_cnt[1] == 2) {
      valid_cnt[1] = 0;
      delta_buffer[1][delta_index[1]] = 0xFFFF;
      if (++delta_index[1] >= CAPTURE_BUFFER_SIZE) { delta_index[1] = 0; }
    }
  }

  // TB0 Capture
  if (TB0CN & 0x40) {
    TB0CN &= ~0x40;
    handle_capture(0);

    if (TB0CN & 0x40)
      isr_overflow++;
  }

  // TB1 Capture
  if (TB1CN & 0x40) {
    TB1CN &= ~0x40;
    handle_capture(1);

    if (TB1CN & 0x40)
      isr_overflow++;
  }
}
